# -*- coding: utf-8 -*-
from naoqi import *
import time
import math

robotIP="192.168.1.11"
PORT=9559

a1=True
a2=True
a3=True

motionProxy = ALProxy("ALMotion", robotIP, PORT)
postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

motionProxy.wakeUp()
postureProxy.goToPosture("StandInit", 0.5)

motionProxy.moveTo(0, 0, 0)
y=0

if (a1 ==True):
    y=1
    # t=-1*math.pi/4
    t=0
    motionProxy.moveTo(1.5, y, t)
    i1=0
elif(a1 ==False):
    y = -1
    # t = math.pi / 4
    t=0
    motionProxy.moveTo(1.5, y, t)
    i1=1


if(i1==0 and a2 == True):
    y=0
    t = 0
    i2=0
    motionProxy.moveTo(1.5, y, t)
elif(i1==1 and a2 == True):
    y=2
    t = 0
    i2 = 0
    motionProxy.moveTo(1.5, y, t)
elif(i1==0 and a2 == False):
    y=-2
    t = 0
    i2 = 1
    motionProxy.moveTo(1.5, y, t)
elif(i1 == 1 and a2 == False):
    y = 0
    t = 0
    i2 = 1
    motionProxy.moveTo(1.5, y, t)

if(i2==0 and a3 == True):
    y=0
    t = 0
    i3=0
    motionProxy.moveTo(1.5, y, t)
elif(i2==1 and a3 == True):
    y=2
    t = 0
    i3 = 0
    motionProxy.moveTo(1.5, y, t)
elif(i2==0 and a3 == False):
    y=-2
    t = 0
    i3 = 1
    motionProxy.moveTo(1.5, y, t)
elif(i2 == 1 and a3 == False):
    y = 0
    t = 0
    i3 = 1
    motionProxy.moveTo(1.5, y, t)
elif(i3 == 1 or i3 == 0):
    y = 0
    t = 0
    motionProxy.moveTo(1.5, y, t)

